#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
云台控制功能函数
Gimbal Control Functions

为LLM提供云台控制的Function Calling接口
基于现有的SimpleServoController实现
"""

import logging
from typing import Dict, Any, Optional

try:
    from ..servo_control.servo_controller import SimpleServoController
    from .function_registry import FunctionRegistry
except ImportError:
    import sys
    import os
    # 当直接运行时添加路径
    current_dir = os.path.dirname(os.path.abspath(__file__))
    servo_control_dir = os.path.join(current_dir, '..', 'servo_control')
    sys.path.insert(0, servo_control_dir)
    
    from servo_controller import SimpleServoController
    from function_registry import FunctionRegistry

logger = logging.getLogger(__name__)

# 云台限位设置 (单位: 度)
GIMBAL_LIMIT_HOR_MIN = 30
GIMBAL_LIMIT_HOR_MAX = 200
GIMBAL_LIMIT_VER_MIN = 30
GIMBAL_LIMIT_VER_MAX = 200

# 全局云台控制器实例
_gimbal_controller: Optional[SimpleServoController] = None


def validate_gimbal_angles(pan: float, tilt: float) -> tuple[bool, str]:
    """
    验证云台角度是否在限位范围内
    
    Args:
        pan: 水平角度 (度)
        tilt: 垂直角度 (度)
        
    Returns:
        tuple: (是否有效, 错误信息)
    """
    if not (GIMBAL_LIMIT_HOR_MIN <= pan <= GIMBAL_LIMIT_HOR_MAX):
        return False, f"云台水平角度超出限位范围 {pan}° (有效范围: {GIMBAL_LIMIT_HOR_MIN}-{GIMBAL_LIMIT_HOR_MAX}°)"
    if not (GIMBAL_LIMIT_VER_MIN <= tilt <= GIMBAL_LIMIT_VER_MAX):
        return False, f"云台垂直角度超出限位范围 {tilt}° (有效范围: {GIMBAL_LIMIT_VER_MIN}-{GIMBAL_LIMIT_VER_MAX}°)"
    return True, ""


def get_gimbal_controller() -> SimpleServoController:
    """获取云台控制器实例"""
    global _gimbal_controller
    if _gimbal_controller is None:
        raise RuntimeError("云台控制器未初始化，请先调用initialize_gimbal函数")
    return _gimbal_controller


def initialize_gimbal_controller(port: str = '/dev/ttyS1', baudrate: int = 115200,
                               pan_pin: int = 3, tilt_pin: int = 4) -> bool:
    """
    初始化云台控制器
    
    Args:
        port: 串口设备路径
        baudrate: 波特率
        pan_pin: 水平舵机引脚
        tilt_pin: 垂直舵机引脚
        
    Returns:
        bool: 是否成功初始化
    """
    global _gimbal_controller
    
    try:
        _gimbal_controller = SimpleServoController(
            port=port,
            baudrate=baudrate,
            pan_pin=pan_pin,
            tilt_pin=tilt_pin
        )
        logger.info(f"云台控制器初始化成功: {port}")
        return True
    except Exception as e:
        logger.error(f"云台控制器初始化失败: {e}")
        _gimbal_controller = None
        return False


@FunctionRegistry.register_function('gimbal_control', '控制云台转动到指定的绝对角度位置')
def gimbal_control_func(pan: float, tilt: float) -> str:
    """
    控制云台到指定绝对角度
    
    这个工具函数可以精确控制二自由度云台的水平和垂直角度。
    
    **重要限位信息：**
    - 实际可用角度范围：水平30-200°，垂直30-200°（安全限位）
    - 机械角度范围：0-270°（仅供参考，请使用限位范围）
    - 中心位置：135°（水平和垂直）
    
    **角落位置建议（在限位范围内）：**
    - 左上角：推荐 pan=50°, tilt=170°
    - 左下角：推荐 pan=50°, tilt=50°  
    - 右上角：推荐 pan=180°, tilt=170°
    - 右下角：推荐 pan=180°, tilt=50°
    
    Args:
        pan: 水平角度，**必须在30-200度范围内**（30度=偏左，135度=中心，200度=偏右）
        tilt: 垂直角度，**必须在30-200度范围内**（30度=偏下，135度=中心，200度=偏上）
        
    Returns:
        str: 执行结果描述
        
    使用示例：
        - 转向左上角：gimbal_control(50, 170)
        - 回到中心位置：gimbal_control(135, 135)
        - 转向右下角：gimbal_control(180, 50)
    """
    try:
        # 参数验证
        if not isinstance(pan, (int, float)) or not isinstance(tilt, (int, float)):
            return "错误：角度参数必须是数字"
        
        # 限位检查 - 使用更严格的限位范围
        is_valid, error_msg = validate_gimbal_angles(pan, tilt)
        if not is_valid:
            return f"限位错误：{error_msg}。请调整目标角度到安全范围内。"
        
        # 原有的0-270度范围检查作为额外保护
        if not (0 <= pan <= 270):
            return f"错误：水平角度超出机械范围 {pan}° (机械范围: 0-270度)"
        
        if not (0 <= tilt <= 270):
            return f"错误：垂直角度超出机械范围 {tilt}° (机械范围: 0-270度)"
        
        controller = get_gimbal_controller()
        
        # 如果未连接，尝试连接
        if not controller.connected:
            if not controller.connect():
                return "错误：云台连接失败，请检查硬件连接和串口配置"
        
        # 执行控制
        success = controller.move_to_angle(float(pan), float(tilt))
        
        if success:
            current_pan, current_tilt = controller.get_current_angles()
            return f"云台控制成功：已移动到 水平={current_pan:.1f}°, 垂直={current_tilt:.1f}°"
        else:
            return "错误：云台移动失败，请检查硬件状态"
            
    except RuntimeError as e:
        return f"错误：{str(e)}"
    except Exception as e:
        logger.error(f"云台控制异常: {e}")
        return f"云台控制过程中出现错误：{str(e)}"


@FunctionRegistry.register_function('gimbal_move_relative', '相对移动云台，在当前位置基础上调整角度')
def gimbal_move_relative_func(pan_delta: float = 0, tilt_delta: float = 0) -> str:
    """
    相对移动云台
    
    这个工具函数用于微调云台位置，在当前角度基础上进行相对移动。
    使用直观的方向语义，正值表示增加角度方向。
    
    Args:
        pan_delta: 水平角度变化量（正值向右，负值向左）
        tilt_delta: 垂直角度变化量（正值向上，负值向下）
        
    Returns:
        str: 执行结果描述
        
    使用示例：
        - 向右上微调：gimbal_move_relative(10, 10)
        - 向左下微调：gimbal_move_relative(-10, -10)
        - 只向右调整：gimbal_move_relative(15, 0)
    """
    try:
        # 参数验证
        if not isinstance(pan_delta, (int, float)) or not isinstance(tilt_delta, (int, float)):
            return "错误：角度变化量参数必须是数字"
        
        controller = get_gimbal_controller()
        
        # 如果未连接，尝试连接
        if not controller.connected:
            if not controller.connect():
                return "错误：云台连接失败，请检查硬件连接和串口配置"
        
        # 获取当前角度并计算目标角度
        current_pan, current_tilt = controller.get_current_angles()
        target_pan = current_pan + pan_delta  # 正值向右，负值向左（直观语义）
        target_tilt = current_tilt + tilt_delta  # 正值向上，负值向下
        
        # 限位检查
        is_valid, error_msg = validate_gimbal_angles(target_pan, target_tilt)
        if not is_valid:
            return f"限位错误：{error_msg}。当前位置：水平={current_pan:.1f}°, 垂直={current_tilt:.1f}°，无法执行该移动。"
        
        # 机械范围检查
        if not (0 <= target_pan <= 270) or not (0 <= target_tilt <= 270):
            return f"错误：目标角度超出机械范围。目标位置：水平={target_pan:.1f}°, 垂直={target_tilt:.1f}° (机械范围: 0-270°)"
        
        # 使用绝对移动执行相对移动（避免双重方向转换）
        success = controller.move_to_angle(target_pan, target_tilt)
        
        if success:
            current_pan, current_tilt = controller.get_current_angles()
            return f"云台相对移动成功：当前位置 水平={current_pan:.1f}°, 垂直={current_tilt:.1f}°"
        else:
            return "错误：云台相对移动失败，请检查硬件状态"
            
    except RuntimeError as e:
        return f"错误：{str(e)}"
    except Exception as e:
        logger.error(f"云台相对移动异常: {e}")
        return f"云台相对移动过程中出现错误：{str(e)}"


@FunctionRegistry.register_function('gimbal_reset', '重置云台到初始中心位置')
def gimbal_reset_func() -> str:
    """
    重置云台到初始中心位置
    
    这个工具函数会将云台重置到预设的初始位置（135°, 135°），
    这是云台的机械中心位置，提供最大的运动范围。
    
    Returns:
        str: 执行结果描述
        
    使用场景：
        - 系统初始化时的位置校准
        - 控制异常后的位置复位
        - 手动控制后的快速回中
    """
    try:
        controller = get_gimbal_controller()
        
        # 如果未连接，尝试连接
        if not controller.connected:
            if not controller.connect():
                return "错误：云台连接失败，请检查硬件连接和串口配置"
        
        # 执行重置
        success = controller.reset_position()
        
        if success:
            current_pan, current_tilt = controller.get_current_angles()
            return f"云台重置成功：已回到中心位置 水平={current_pan:.1f}°, 垂直={current_tilt:.1f}°"
        else:
            return "错误：云台重置失败，请检查硬件状态"
            
    except RuntimeError as e:
        return f"错误：{str(e)}"
    except Exception as e:
        logger.error(f"云台重置异常: {e}")
        return f"云台重置过程中出现错误：{str(e)}"


@FunctionRegistry.register_function('gimbal_status', '获取云台当前状态信息')
def gimbal_status_func() -> str:
    """
    获取云台当前状态信息
    
    这个工具函数返回云台的详细状态信息，包括：
    - 当前的水平和垂直角度
    - 云台连接状态
    - 配置信息
    - 控制统计数据
    
    Returns:
        str: 状态信息描述
    """
    try:
        controller = get_gimbal_controller()
        
        # 获取状态
        status = controller.get_status()
        
        # 构建友好的状态描述
        result_parts = []
        
        # 连接状态
        connection_status = "已连接" if status['connected'] else "未连接"
        result_parts.append(f"云台连接状态：{connection_status}")
        
        # 当前角度
        angles = status['current_angles']
        result_parts.append(f"当前角度：水平={angles['pan']:.1f}°, 垂直={angles['tilt']:.1f}°")
        
        # 限位信息
        result_parts.append(f"限位范围：水平={GIMBAL_LIMIT_HOR_MIN}-{GIMBAL_LIMIT_HOR_MAX}°, 垂直={GIMBAL_LIMIT_VER_MIN}-{GIMBAL_LIMIT_VER_MAX}°")
        result_parts.append(f"机械范围：水平/垂直=0-270°")
        
        # 配置信息
        config = status['config']
        result_parts.append(f"配置信息：串口={config['serial_port']}, 波特率={config['baudrate']}")
        result_parts.append(f"舵机引脚：水平={config['pan_pin']}, 垂直={config['tilt_pin']}")
        
        # 统计信息
        stats = status['stats']
        total_commands = stats['total_commands']
        successful_commands = stats['successful_commands']
        failed_commands = stats['failed_commands']
        
        if total_commands > 0:
            success_rate = (successful_commands / total_commands) * 100
            result_parts.append(f"控制统计：总命令={total_commands}, 成功={successful_commands}, 失败={failed_commands}")
            result_parts.append(f"成功率：{success_rate:.1f}%")
        else:
            result_parts.append("控制统计：暂无执行记录")
        
        return "\n".join(result_parts)
        
    except RuntimeError as e:
        return f"错误：{str(e)}"
    except Exception as e:
        logger.error(f"获取云台状态异常: {e}")
        return f"获取云台状态过程中出现错误：{str(e)}"


@FunctionRegistry.register_function('gimbal_connect', '连接云台控制器')
def gimbal_connect_func() -> str:
    """
    连接云台控制器
    
    这个工具函数会尝试连接到云台硬件设备，建立串口通信。
    
    Returns:
        str: 连接结果描述
        
    使用场景：
        - 系统启动时的硬件连接
        - 连接断开后的重新连接
        - 硬件状态检查
    """
    try:
        controller = get_gimbal_controller()
        
        if controller.connected:
            return "云台已连接"
        
        success = controller.connect()
        
        if success:
            return "云台连接成功"
        else:
            return "云台连接失败，请检查硬件连接和串口配置"
            
    except RuntimeError as e:
        return f"错误：{str(e)}"
    except Exception as e:
        logger.error(f"云台连接异常: {e}")
        return f"云台连接过程中出现错误：{str(e)}"


@FunctionRegistry.register_function('gimbal_disconnect', '断开云台控制器连接')
def gimbal_disconnect_func() -> str:
    """
    断开云台控制器连接
    
    这个工具函数会安全地断开与云台硬件设备的连接。
    
    Returns:
        str: 断开结果描述
        
    使用场景：
        - 系统关闭前的安全断开
        - 硬件维护前的断开连接
        - 连接异常后的强制断开
    """
    try:
        controller = get_gimbal_controller()
        
        if not controller.connected:
            return "云台未连接"
        
        controller.disconnect()
        
        if not controller.connected:
            return "云台连接已断开"
        else:
            return "云台断开连接失败"
            
    except RuntimeError as e:
        return f"错误：{str(e)}"
    except Exception as e:
        logger.error(f"云台断开连接异常: {e}")
        return f"云台断开连接过程中出现错误：{str(e)}"